#include "param_storage.h"
#include "stm32f1xx_hal.h"
#include "cmsis_os.h"
#include <string.h>
#include "usr_protocol.h"


// Magic number for parameter validation
#define PARAM_MAGIC          0xA5B6C7D8

// Flash page size for STM32F103
#define FLASH_PAGE_SIZE     0x400    // 1KB



// Static variables
static uint8_t param_modified = 0;


// Forward declarations of private functions
uint32_t Calc_CRC32(const uint8_t* data, uint32_t length);
 HAL_StatusTypeDef Flash_Write_Data(uint32_t addr, const uint8_t* data, uint32_t length);
 HAL_StatusTypeDef Flash_Erase_Pages(uint32_t start_addr, uint32_t end_addr);


void init_control_board_params(void)
{
    
    // Initialize default values for both groups
    for(uint8_t group = 0; group < 2; group++) {

        motor_status_reg_map[group][BROADCAST_POSITIVE_TORQUE] = 13288;
        motor_status_reg_map[group][BROADCAST_POSITIVE_TORQUE] = -13288;
        // Set control parameters
        control_board_param_reg_map[group][BROADCAST_IP_1-CONTROL_BOARD_PARAM_BASE] = 192u;     // Control register
        control_board_param_reg_map[group][BROADCAST_IP_2-CONTROL_BOARD_PARAM_BASE] = 168u; 
        control_board_param_reg_map[group][BROADCAST_IP_3-CONTROL_BOARD_PARAM_BASE] = 1u;  
        control_board_param_reg_map[group][BROADCAST_IP_4-CONTROL_BOARD_PARAM_BASE] = 199u; // Control register

        control_board_param_reg_map[group][BROADCAST_MAX_SPEED - CONTROL_BOARD_PARAM_BASE ] = 2048;
        control_board_param_reg_map[group][CONTROL_BOARD_DEVICE_NUM - CONTROL_BOARD_PARAM_BASE] = 64; // Device number


        for(int id =0; id<MAX_ID; id++)
        {
           

        }

    }
}


HAL_StatusTypeDef Param_Load(void)
{
    ParamHeader_t header;

    // Read header
    memcpy(&header, (void*)PARAM_FLASH_START_ADDR, sizeof(ParamHeader_t));

    // Check magic number
    if (header.magic != PARAM_MAGIC) {
        return Param_Reset();
    }

    // Check version
    if (header.version != PARAM_VERSION) {
        return Param_Reset();
    }


    // Calculate CRC of data
    uint32_t calc_crc = Calc_CRC32((uint8_t*)(PARAM_FLASH_START_ADDR + sizeof(ParamHeader_t)), 
    sizeof(control_board_param_reg_map));

    // Verify CRC
    if (calc_crc != header.crc) {
    return Param_Reset();
    }

    // Load data
    uint32_t data_addr = PARAM_FLASH_START_ADDR + sizeof(ParamHeader_t);

    // Load control board parameters
    memcpy(control_board_param_reg_map, (void*)data_addr, sizeof(control_board_param_reg_map));
    data_addr += sizeof(control_board_param_reg_map);
    
    // Load motor status parameters
    memcpy(motor_status_reg_map, (void*)data_addr, sizeof(motor_status_reg_map));

    return HAL_OK;

}

HAL_StatusTypeDef Param_Save(void)
{
    ParamHeader_t header;
    HAL_StatusTypeDef status;
    uint32_t primask_state;
  
    // 保存当前中断状态
    primask_state = __get_PRIMASK();
    __disable_irq();


    // Fill header
    header.magic = PARAM_MAGIC;
    header.version = PARAM_VERSION;
    header.crc = Calc_CRC32((uint8_t*)control_board_param_reg_map,
                         sizeof(control_board_param_reg_map));
    header.reserved = 0;

    // Erase Flash pages
    status = Flash_Erase_Pages(PARAM_FLASH_START_ADDR, PARAM_FLASH_END_ADDR);
    if (status != HAL_OK) {
        return status;
    }

    // Write header
    status = Flash_Write_Data(PARAM_FLASH_START_ADDR, (uint8_t*)&header, sizeof(header));
    if (status != HAL_OK) {
        return status;
    }

    // Write parameter data
    uint32_t data_addr = PARAM_FLASH_START_ADDR + sizeof(ParamHeader_t);


    // Write control board parameters
    status = Flash_Write_Data(data_addr, (uint8_t*)control_board_param_reg_map, 
                            sizeof(control_board_param_reg_map));
    if (status != HAL_OK) {
        return status;
    }

    // Update data address
    data_addr += sizeof(control_board_param_reg_map);

    // Write motor status parameters
    status = Flash_Write_Data(data_addr, (uint8_t*)motor_status_reg_map,
                            sizeof(motor_status_reg_map));

  
    // 恢复之前的中断状态
    if (!primask_state) {
        __enable_irq();
    }

    return status;
}

HAL_StatusTypeDef Param_Reset(void)
{
    init_control_board_params();
    // Save to Flash
    return Param_Save();
}

void Param_SetModified(void)
{
    param_modified = 1;
}

// Private function implementations
uint32_t Calc_CRC32(const uint8_t* data, uint32_t length)
{
    uint32_t crc = 0xFFFFFFFF;

    for (uint32_t i = 0; i < length; i++) {
        crc ^= data[i];
        for (uint32_t j = 0; j < 8; j++) {
            if (crc & 1) {
                crc = (crc >> 1) ^ 0xEDB88320;
            } else {
                crc >>= 1;
            }
        }
    }

    return ~crc;
}


 HAL_StatusTypeDef Flash_Write_Data(uint32_t addr, const uint8_t* data, uint32_t length)
{
    HAL_StatusTypeDef status;

    // Unlock Flash
    HAL_FLASH_Unlock();

    // Write data
    for (uint32_t i = 0; i < length; i += 2) {
        uint16_t halfword = data[i];
        if (i + 1 < length) {
            halfword |= (data[i + 1] << 8);
        }
        
        status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_HALFWORD, addr + i, halfword);
        if (status != HAL_OK) {
            HAL_FLASH_Lock();
            return status;
        }
    }

    // Lock Flash
    HAL_FLASH_Lock();

    return HAL_OK;
}

 HAL_StatusTypeDef Flash_Erase_Pages(uint32_t start_addr, uint32_t end_addr)
{
    HAL_StatusTypeDef status;
    FLASH_EraseInitTypeDef erase_init;
    uint32_t page_error;

    // Calculate number of pages
    uint32_t num_pages = (end_addr - start_addr) / FLASH_PAGE_SIZE + 1;

    // Configure erase operation
    erase_init.TypeErase = FLASH_TYPEERASE_PAGES;
    erase_init.PageAddress = start_addr;
    erase_init.NbPages = num_pages;

    // Unlock Flash
    HAL_FLASH_Unlock();

    // Erase pages
    status = HAL_FLASHEx_Erase(&erase_init, &page_error);

    // Lock Flash
    HAL_FLASH_Lock();

    return status;
}